/*
 * smartieparts-telemetry
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 */ 


#include <avr/io.h>
#include <avr/interrupt.h>

#define PINSTATUS_AILEDR PINC1
#define PINSTATUS_THRCT PINC3

#define frskyDataIdle    0
#define frskyDataStart   1
#define frskyDataInFrame 2

#define START_STOP      0x7e
#define SWITCHPKT		0x70

void USART1_Transmit( uint8_t data ) //transmit to 9x
{
	while ( !( UCSR1A & (1<<UDRE1)) );
	UDR1 = data;
}

void USART0_Transmit( uint8_t data ) //transmit to FrSky
{
	while ( !( UCSR0A & (1<<UDRE0)) );
	UDR0 = data;
}

void forwardFrskyPacket(uint8_t *packet, uint8_t length) //transmit unmodified frsky packet to 9x
{
	for(uint8_t i=0;i<=length;i++)
	{
		USART1_Transmit(packet[i]);
	}
}

void transmitSwitchStatus(uint8_t AileDR, uint8_t ThrCt) //transmit custom packet to 9x
{
	//byte stuffing should not be necessary but if it's needed, add it!
	sei();
	USART1_Transmit(START_STOP);
	USART1_Transmit(SWITCHPKT);
	USART1_Transmit(AileDR);
	USART1_Transmit(ThrCt);
	for(uint8_t i=0;i<6;i++)
	{
		USART1_Transmit(0x00);
	}		
	USART1_Transmit(START_STOP);
	cli();
}

int main(void)
{
	//change to 8mhz regardless of fuses
	CCP = 0xD8;
	CLKPR = 0x00;
	
	//set timer 1 at /1024
	TCCR1B |= ((1 << CS10) | (1 << CS12));
	
	/*
	define pin functions
	PortC1 = AileDR = Pin14
	PortC3 = ThrCt = Pin12
	*/
	DDRC = 0x00; PORTC = 0x00;
	
	/*
	USART1 = 9X USART0 = FrSky
	
	USART0 RX = PortA7 = Pin20
	USART0 TX = PortB0 = Pin19
	USART1 RX = PortB1 = Pin18
	USART1 TX = PortB2 = Pin17
	*/
	/* Set baud rate */
	uint8_t baud = 51; //8mhz internal
	UBRR0H = (unsigned char)(baud>>8);
	UBRR0L = (unsigned char)baud;
	UBRR1H = (unsigned char)(baud>>8);
	UBRR1L = (unsigned char)baud;
	/* Enable receiver and transmitter */
	UCSR0B = (1<<RXEN0)|(1<<TXEN0);
	UCSR1B = (1<<RXEN1)|(1<<TXEN1);
	/* Set frame format: 8data, 1stop bit */
	UCSR0C = (0<<USBS0)|(3<<UCSZ00);
	UCSR1C = (0<<USBS1)|(3<<UCSZ10);
	
	uint8_t SW_AileDR, SW_ThrCt;
	
    while(1)
    {
		//incoming serial data from frsky is handled by interrupt
		//read switch status, write serial outputs
		if (TCNT1 >= 1000) //roughly 8Hz
		{
			SW_AileDR = PINSTATUS_AILEDR;
			SW_ThrCt = PINSTATUS_THRCT;
			transmitSwitchStatus(SW_AileDR,SW_ThrCt);
			TCNT1 = 0;
		}
    }
}

ISR(USART0__RX_vect) //byte received from FrSky
{
	uint8_t stat;
	uint8_t data;
	
	static uint8_t frskyRxBuffer[19];
	static uint8_t dataState = frskyDataIdle;
	static uint8_t numPktBytes = 0;
  
	UCSR0B &= ~(1 << RXCIE0); // disable Interrupt
	sei() ;
	stat = UCSR0A; // USART control and Status Register 0 A

    /*
              bit      7      6      5      4      3      2      1      0
                      RxC0  TxC0  UDRE0    FE0   DOR0   UPE0   U2X0  MPCM0
             
              RxC0:   Receive complete
              TXC0:   Transmit Complete
              UDRE0:  USART Data Register Empty
              FE0:    Frame Error
              DOR0:   Data OverRun
              UPE0:   USART Parity Error
              U2X0:   Double Tx Speed
              PCM0:   MultiProcessor Comms Mode
    */
    // rh = UCSR0B; //USART control and Status Register 0 B

    /*
              bit      7      6      5      4      3      2      1      0
                   RXCIE0 TxCIE0 UDRIE0  RXEN0  TXEN0 UCSZ02  RXB80  TXB80
             
              RxCIE0:   Receive Complete int enable
              TXCIE0:   Transmit Complete int enable
              UDRIE0:   USART Data Register Empty int enable
              RXEN0:    Rx Enable
              TXEN0:    Tx Enable
              UCSZ02:   Character Size bit 2
              RXB80:    Rx data bit 8
              TXB80:    Tx data bit 8
    */

  data = UDR0; // USART data register 0

  if (stat & ((1 << FE0) | (1 << DOR0) | (1 << UPE0)))
  { // discard buffer and start fresh on any comms error
	  numPktBytes = 0;
  }	  
  else
  {
	  switch (dataState) 
      {
	  case frskyDataStart:
          if (data == START_STOP) break; // Remain in userDataStart if possible 0x7e,0x7e doublet found.
          if (numPktBytes < 19)
			frskyRxBuffer[numPktBytes++] = data;
          dataState = frskyDataInFrame;
          break;
      case frskyDataInFrame:
          if (data == START_STOP) // end of frame detected
          {
			  frskyRxBuffer[numPktBytes++] = data;
			  forwardFrskyPacket(frskyRxBuffer, numPktBytes);
			  dataState = frskyDataIdle;
			  break;
          }
		  else
		  {
			  frskyRxBuffer[numPktBytes++] = data;
		  }			  
          break;
      case frskyDataIdle:
          if (data == START_STOP)
		  {
			  numPktBytes = 0;
			  frskyRxBuffer[numPktBytes++] = data;
			  dataState = frskyDataStart;
		  }
          break;
      } // switch
  }
  cli() ;
  UCSR0B |= (1 << RXCIE0); // enable Interrupt
}

ISR(USART1__RX_vect) //9x->frsky forwarding
{
	UCSR1B &= ~(1 << RXCIE1); // disable Interrupt
	sei() ;
	
	uint8_t data = UDR1;
	USART0_Transmit(data);
	
	cli() ;
	UCSR1B |= (1 << RXCIE1); // enable Interrupt
}

